#include "position.h"
#include "stdio.h"


// 用于计算位置环两次PID的时间差
volatile uint32_t gPositionPID_TimeCNT;


/**
 * @brief   此函数放在滴答定时器中断中运行，即每次运行+1
 *
 * @param   ms		需要增加的时间（单位ms）
 *
 * @return  none
 */
void POSITION_PID_TIMEBASE(uint16_t ms)
{
	gPositionPID_TimeCNT += ms;
}

/**
 * @brief   获取此时的计数值
 *
 * @param   none
 *
 * @return  gSpeedPID_TimeCNT
 */
uint32_t POSITION_PID_GETTIME(void)
{
	return gPositionPID_TimeCNT;
}

/**
 * @brief   判断计数值是否大于上次计数值（用于计算周期时的越限判断）
 *
 * @param   none
 *
 * @return  gSpeedPID_TimeCNT
 */
uint8_t POSITION_PID_TIMEOUT(float timeOut, float startTime)
{
	if((gPositionPID_TimeCNT - startTime) > timeOut)
		return 1;
	else
		return 0;
}

/* ================================================================ */

/**
 * @brief   位置环PID控制
 *
 * @param   pPositionPID		pPositionPID结构体指针
 *
 * @return  none
 */
static void PositionPIDControl(PPOSITION_Control_Struct pPositionPID)
{
	float pre;
	
	// 1. 实际值
    pPositionPID->pPID.pre = pPositionPID->GetPosition();
	
	/* 调试信息输出（经过变换之后的dq应该是一个常量） */
	PositionPIDPrintf(pPositionPID);
	
	// 2. 旋转坐标系
	pre = pPositionPID->pPID.pre - pPositionPID->pPID.tar;
	if(pre > 180)
	{
		pre -= 360;
	}
	else if (pre < -180)
	{
		pre += 360;
	}
	// 3. 计算偏差
    pPositionPID->pPID.bias = 0 - pre;
	// 4. 计算输出（位置式:out = kp * 偏差 + ki * 累计误差 + kd * （本次误差 - 上次误差））
    pPositionPID->pPID.out = pPositionPID->pPID.kp * pPositionPID->pPID.bias + pPositionPID->pPID.kd * (pPositionPID->pPID.bias - pPositionPID->pPID.lastBias) +  pPositionPID->pPID.ki * pPositionPID->pPID.err;
    // 5. 更新误差
	pPositionPID->pPID.lastBias = pPositionPID->pPID.bias;
	// 6. 误差积分
    pPositionPID->pPID.err += pPositionPID->pPID.bias;
	// 7. 输出限幅
	if (pPositionPID->pPID.out > pPositionPID->pPID.outMax)
	{
		pPositionPID->pPID.out = pPositionPID->pPID.outMax;
	} 
	else if (pPositionPID->pPID.out < -pPositionPID->pPID.outMax)
	{
		pPositionPID->pPID.out = -pPositionPID->pPID.outMax;
	}
	// 8. 使能判断
    if(pPositionPID->isEnable == 0)
	{
        pPositionPID->pPID.out = 0;
    }
	// 9. 设置输出
    pPositionPID->SetMotorSpeed(pPositionPID->pPID.out);
}

/**
 * @brief   位置环周期控制
 *
 * @param   pPositionPID		pPositionPID结构体指针
 *
 * @return  none
 */
void PositionPIDCycle(PPOSITION_Control_Struct pPositionPID)
{
    if(POSITION_PID_TIMEOUT(pPositionPID->pPID.cycle * 1000.0f, pPositionPID->sysTimer))
	{
        pPositionPID->sysTimer = POSITION_PID_GETTIME();
        PositionPIDControl(pPositionPID);
    }
}

/**
 * @brief   设置位置环pid参数
 *
 * @param   pPositionPID	pPositionPID结构体指针
 * @param   kp				比例
 * @param   ki				积分
 * @param   kd				微分
 *
 * @return  none
 */
void SetPositionPIDParams(PPOSITION_Control_Struct pPositionPID, float kp, float ki, float kd)
{
    pPositionPID->pPID.kp = kp;
    pPositionPID->pPID.ki = ki;
    pPositionPID->pPID.kd = kd;
}

/**
 * @brief   设置位置环目标值
 *
 * @param   pPositionPID	pPositionPID结构体指针
 * @param   tar				位置环目标值
 *
 * @return  none
 */
void SetPositionPIDTar(PPOSITION_Control_Struct pPositionPID, float tar)
{
    pPositionPID->pPID.tar = tar;
}

/**
 * @brief   位置环使能
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 * @param   isEnable		0：失能；1：使能
 *
 * @return  none
 */
void SetPositionPIDEnable(PPOSITION_Control_Struct pPositionPID, float isEnable)
{
    pPositionPID->isEnable = isEnable;
}

/**
 * @brief   调试信息输出
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 *
 * @return  none
 */
#include "vofa.h"
void PositionPIDPrintf(PPOSITION_Control_Struct pPositionPID)
{
	float ch_data[5];
	
	// 调试信息输出（数据顺序为：实际位置、目标位置、kp、ki、kd）
	ch_data[0] = pPositionPID->pPID.pre;
	ch_data[1] = pPositionPID->pPID.tar;
	
	ch_data[2] = pPositionPID->pPID.kp;
	ch_data[3] = pPositionPID->pPID.ki;
	ch_data[4] = pPositionPID->pPID.kd;
	VOFA_JUSTFLOAT_SEND(ch_data, sizeof(ch_data)/sizeof(float));
}

